/// 通过ceres构建求解相机参数  3个旋转角度 + 3个平移 + f + u0,v0
/// Cr_RigidTransform.cpp 中可以求出 两个坐标之间的转化矩阵
/// TODO: 这里构造了好几个点，都无法求出 f, u, v
///
#include "ceres/ceres.h"
#include "glog/logging.h"

#include <memath/meTransform.h>
#include <memath/MeEigenMath.h>

using namespace std;

// 数据来自于一影采集的2D图像，图像大小为2064*2064
// 二维标尺有上下两个面，采集两次得到4个面的数据
const static double s_objPoints[] = {
    33.50, 73.77, 78,
    -3.27, 37, 78,
    33.50, 0.23, 78,
    70.27, 37, 78,
    33.50, 63.77, 78,
    // plane2
    67, 7, 2,
    0, 0, 2,
    0, 74, 2,
    67, 67, 2,
    71.95, 2.05, 2,
    //55.06, 5.75, 2,
    // plane3
    33.50, 73.77, 78,
    -3.27, 37, 78,
    33.50, 0.23, 78,
    70.27, 37, 78,
    33.50, 63.77, 78,
    // plane4
    67, 7, 2,
    0, 0, 2,
    0, 74, 2,
    67, 67, 2,
    71.95, 2.05, 2,
};

const static double s_imagePoints[] = {
    558.791 ,1481.58,
    175.822 ,901.062,
    754.578 ,534.524,
    1130.44 ,1109.1,
    585.582 ,1352.91,
    // plane2
    1317.02 ,573.621,
    386.046 ,264.715,
    166.893 ,1322.2,
    1147.49 ,1423.43,
    1401.28 ,520.14,
    //1152.33 ,519.293,

    // plane3
    1290.68 ,1513.81,
    783.605 ,1045.01,
    1254.05 ,542.663,
    1757.41 ,1011.03,
    1286.35 ,1381.44,
    // plane4
    1782.11 ,544.629,
    801.412 ,479.553,
    845.876 ,1559.89,
    1818.47 ,1418.17,
    1851.84 ,470.476,
    //1607.66 ,532.807,
};


struct ProjMCostFunction {
    ProjMCostFunction(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2):
        pI(p1), pO(p2){}
    template <typename T>
    bool operator()(const T* const parameters,
                    T* residuals) const {
        const T& x = parameters[0];
        const T& y = parameters[1];
        const T& z = parameters[2];

        const T& t1 = parameters[3];
        const T& t2 = parameters[4];
        const T& t3 = parameters[5];

        const T& f = parameters[6];
        const T& u = parameters[7];
        const T& v = parameters[8];
        //double u = 0, v=0;

        T px = cos(y)*cos(z)*pI[0] + (sin(x)*sin(y)*cos(z) - cos(x)*sin(z))*pI[1] + (cos(x)*sin(y)*cos(z) + sin(x)*sin(z))*pI[2] + t1;
        T py = cos(y)*sin(z)*pI[0] + (sin(x)*sin(y)*sin(z) + cos(x)*cos(z))*pI[1] + (cos(x)*sin(y)*sin(z) - sin(x)*cos(z))*pI[2] + t2;
        T pz = -sin(y)*pI[0] + (sin(x)*cos(y))*pI[1] + (cos(x)*cos(y))*pI[2] + t3;

//        residuals[0] = px*f/pz + u - pO[0];
//        residuals[1] = py*f/pz + v - pO[1];
        residuals[0] = px*f + u *pz - pO[0]*pz;
        residuals[1] = py*f + v *pz - pO[1]*pz;
        // residuals[2] = pz - pO[2];

        return true;
    }

    const Eigen::Vector3d pI;
    const Eigen::Vector3d pO;
};


int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);


    vector<Eigen::Vector3d> p_I = {
        {33.50, 73.77, 78},
        {-3.27, 37, 78},
        {33.50, 0.23, 78},
        {70.27, 37, 78},
        {33.50, 63.77, 78},
        // plane2
        {67, 7, 2},
        {0, 0, 2},
        {0, 74, 2},
        {67, 67, 2},
        {71.95, 2.05, 2},
    };

    vector<Eigen::Vector3d> p_O = {
        {558.791 ,1481.58, 1},
        {175.822 ,901.062, 1},
        {754.578 ,534.524, 1},
        {1130.44 ,1109.1, 1},
        {585.582 ,1352.91, 1},
        // plane2
        {1317.02 ,573.621, 1},
        {386.046 ,264.715, 1},
        {166.893 ,1322.2, 1},
        {1147.49 ,1423.43, 1},
        {1401.28 ,520.14, 1}
    };

    // Build the problem.
    ceres::Problem problem;

    double parameters[9] = {1*ME_DEGREE2RAD, 10*ME_DEGREE2RAD, 20*ME_DEGREE2RAD,
                           0, 0, -500,
                            10000, 1032, 1032};

    // Set up the only cost function (also known as residual).
    for (uint i=0; i<p_I.size();i++) {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ProjMCostFunction, 2, 9>(
                new ProjMCostFunction(p_I[i], p_O[i]));
        problem.AddResidualBlock(cost_function, NULL, parameters);
    }

    // Run the solver!
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;
    options.parameter_tolerance = 1e-16;
    options.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);

    //std::cout << summary.BriefReport() << "\n";
    std::cout << summary.FullReport() << "\n";

    std::cout << "Final   x: " << parameters[0] << " y: " << parameters[1] << " z: " << parameters[2]
              << " t1: " << parameters[3] << " t2: " << parameters[4] << " t3: " << parameters[5]
              << " f: " << parameters[6]  << " u: " << parameters[7]  << " v: " << parameters[8]
              << "\n";

    Matrix4d m2 = MeTransform::rotateZ(parameters[2])
            *MeTransform::rotateY(parameters[1])
            *MeTransform::rotateX(parameters[0]);
    m2(0, 3) = parameters[3];
    m2(1, 3) = parameters[4];
    m2(2, 3) = parameters[5];

    cout << "m2:\n" << m2 << endl;
    return 0;
}
